/*************************************************************************
*
*  compass.c
*
*
*  Copyright 2008,2009 Steve Stancliff
*
*
*  Part of the TAOSF SSA system.
*
*
*  This module reads copmass data from an oceanserver compass.
*
*
*  version 1.1.1, 4/24/09, SBS
*    - documentation
*
*  version 1.1, 1/9/09, SBS
*    - change from callback mode to polled
*
*  version 1.0.1, 1/8/09, SBS
*    - add debug output in GetStatus()
*    - LogPrint --> LogLevelPrint
*
*  version 1.0, 11/19/08, SBS
*   - convert from standalone app to module 
*
*  version 0.1, 11/19/08, SBS
*   - first skeletal version 
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/
/**
*  @defgroup plat_eng_hardware
*	@ingroup plat_engineering
*  Drivers for platform hardware
*/
/**
*  @file 
*  @ingroup plat_eng_hardware
*  @brief
*  This module reads copmass data from an oceanserver compass.
*  <hr>
**/

#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <pthread.h>

#include <stdint.h>
#include "types.h"

#include <time.h>
#include <sys/timeb.h>
#include "delay.h"

#include "waypoints.h"
//#define NEED_MODENAMES
#include "platform_modes.h"
//#undef NEED_MODENAMES
#include "platform_types.h"
#include "status.h"

#ifdef DEBUG
#include "logfile.h"
#endif

#include "cssl.h"


typedef struct{
	float hdgDeg;
	float pitchDeg;
	float rollDeg;
	float tempDegC;
}COMPASS_STATUS;


static COMPASS_STATUS cs;
static cssl_t *serial;


/********************************************************
*
* parse_compass()
*/
/**
* @brief
* Parse string buffer into COMPASS_STATUS struct
*
*
********************************************************/
static void parse_compass(
		char *buf)
{
	char *pS;
	int rval;

// need to add verification of checksum

	pS = strrchr(buf,'$');
	if(pS == NULL){
		printf("parse_compass(): No '$' found\n");
		return;
	}

	rval = sscanf(pS
				,"$C%fP%fR%fT%f*"
				,&(cs.hdgDeg)
				,&(cs.pitchDeg)
				,&(cs.rollDeg)
				,&(cs.tempDegC)
				);
				
	if(rval == EOF){
		printf("parse_compass(): Incomplete string\n");
		return;
	}

	return;
}


/*************************
*
*
*************************/
/*
void compass_callback(
			int id,
			UBYTE *buf,
			int length)
{
//	printf("compass_callback()\n");

//	if(strchr((char *)buf,'$') != NULL){
		parse_compass((char *)buf);
//	}

	return;
}
*/


/********************************************************
*
* compass_GetData()
*/
/**
* @brief
* Fetch data from the cssl library and parse it
*
*
********************************************************/
static void compass_GetData(void)
{
	UBYTE buf[512];
	int length;
	
	length = cssl_getdata(serial,buf,sizeof(buf));

	parse_compass((char *)buf);

	return;
}


/********************************************************
*
* compassInit()
*/
/**
* @brief
* Initialize the cssl library
*
*
********************************************************/
static int compassInit(
			char *port
			,int baud
			,int bits
			,int stopbits)
{
	cs.hdgDeg = 0.;
	cs.pitchDeg = 0.;
	cs.rollDeg = 0.;
	cs.tempDegC = 0.;

	cssl_start();
    
	serial = cssl_open(
				port
//				,compass_callback
				,NULL
				,0
				,baud
				,bits
				,0
				,1);
	
	if (!serial) {
		printf("%s\n",cssl_geterrormsg());
		return(-1);
	}

	cssl_settimeout(serial, 1);

	return(0);
}


/********************************************************
*
* compass_Start()
*/
/**
* @brief
* Called from the main program to set up the compass port
*
*
********************************************************/
int compass_Start(
	char *port)
{
	int rval;

	int baud = 19200;
	int bits = 8;
	int stopbits = 1;
	
	printf("compass_Start()\n");

	#ifdef DEBUG
	logfile_LogLevelPrint(LOGLEVEL_INFO," ");
	logfile_LogLevelPrint(LOGLEVEL_INFO,"*******************************************");		
	logfile_LogLevelPrint(LOGLEVEL_INFO,"COMPASS STARTUP");
	logfile_LogLevelPrint(LOGLEVEL_INFO,"*******************************************");		
	logfile_LogLevelPrint(LOGLEVEL_INFO," ");
	#endif

	rval = compassInit(port,baud,bits,stopbits);

	return(rval);
}


/********************************************************
*
* compass_GetStatus()
*/
/**
* @brief
*  Returns the current compass status from the local 
*    data struct
*
*
********************************************************/
void compass_GetStatus(STATUS *s)
{
	#ifdef DEBUG
	char logtemp[LOGSTRLEN];
	#endif

	compass_GetData();

	sdHdgDeg   = cs.hdgDeg;           
	s->dPitchDeg = cs.pitchDeg;           
	s->dRollDeg  = cs.rollDeg;           

	#ifdef DEBUG
	snprintf(logtemp,LOGSTRLEN,"***** compass_GetStatus() - HPR = %f,%f,%f"
			,cs.hdgDeg
			,cs.pitchDeg
			,cs.rollDeg);		
	logfile_LogLevelPrint(LOGLEVEL_INFO,logtemp);
	#endif


	return;
}

